/********** Local Includes ****************************************************/

#include "simpledrivecontroller.h"

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Macros and Constants **********************************************/

#define DEFAULT_SPEED 4
#define DRIVE_THRESHOLD 30
#define TURN_THRESHOLD 30

/********** Public Methods ****************************************************/

SimpleDriveController::SimpleDriveController() : m_speed(DEFAULT_SPEED) {}

SimpleDriveController::SimpleDriveController(int speed) : m_speed(speed) {}

Command SimpleDriveController::next(RobotOrientation robot,
	Point goal)
{
	double goalAngle;

	if (robot.point.x == goal.x) {
		if (robot.point.y > goal.y) {
			goalAngle = 0;
		} else {
			goalAngle = 180;
		}
	} else {
		double dx = goal.x - robot.point.x;
		double dy = goal.y - robot.point.y;
		
		goalAngle = atan2(dy, dx) / M_PI * 180;
		
		if (goalAngle < 0) {
			goalAngle += 360;
		}
	}
	
	DriveDirection command;
	int time;
	double distance = euclidean(robot.point, goal);
	
	if (distance < DRIVE_THRESHOLD) {
		command = DIR_STOP;
		time = 0;
	} else if (fabs(robot.angle - goalAngle) < TURN_THRESHOLD) {
		command = DIR_FORWARD;
		
		if (distance > 200) {
			time = 2000;
		} else if (distance > 100) {
			time = 1500;
		} else {
			time = 1000;
		}
	//} else if (robot.angle < goalAngle) {
	//	command = DIR_ROT_RIGHT;
	//	time = 200;
	} else {
		command = DIR_ROT_LEFT;
		time = 100;
	}
	
	return Command(command, speed(), time);
}

void SimpleDriveController::setSpeed(int speed) {
	m_speed = speed;
}

int SimpleDriveController::speed() const {
	return m_speed;
}

/********** Private Methods ***************************************************/

double SimpleDriveController::euclidean(Point one, Point two) const {
	double dx = two.x - one.x;
	double dy = two.y - one.y;
	
	return sqrt(dx * dx + dy * dy);
}